package edu.wpi.first.wpilibj.sensor;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.positioning.PositionHandler;

/**
 * Our {@link edu.wpi.first.wpilibj.Gyro} to balance our phlegmy ducttapey thing.
 * @author Dylan Frese
 */
public class Gyro extends edu.wpi.first.wpilibj.Gyro{

    /**
     * {@inheritDoc}
     */
    public Gyro(AnalogChannel channel) {
        super(channel);
    }

    /**
     * {@inheritDoc}
     */
    public Gyro(int channel) {
        super(channel);
    }

    /**
     * {@inheritDoc}
     */
    public Gyro(int slot, int channel) {
        super(slot, channel);
    }
    
    /**
     * Balances the robot on the bridge using the specified
     * {@link RobotDrive}. This will return false, immediately,
     * if the {@link PositionHandler} passed returns 'false'
     * as to the location being on a bridge.
     * The method loops, so it should not be called as an
     * extension to a tick.
     * @param where Where the robot is.
     * @param drive What drive to use to power the robot.
     * @return If the robot is not on a bridge, this will return false
     * immediately. Otherwise, it will return true.
     * 
     */
    public boolean balanceBridge (PositionHandler where, RobotDrive drive){
        if (!where.isOnBridge()){
            return false;
        }
        while (Math.abs(getAngle()) > 5){
            double ang = 360 % getAngle(); //Modulus division. Say the angle is 240.
            //The result would be 240. If it was 540, the result would be 180, or the remainder of 540 / 360.
            ang -= 180; //Angle now corresponds to -180 to 179 range.
            double correctionSpeed = ang * 0.05;
            correctionSpeed = Math.max (Math.min(correctionSpeed, 0.8), 0.2); //Make sure the speed is in the range of 0.2 to 0.8.
            drive.tankDrive(correctionSpeed, correctionSpeed); //Drive!
        }
        return true;
    }
    
}
